// =========================================================================
// ================ smotor.c - 最终修正版 (适配 sj_pid) ================
// =========================================================================

#include "smotor.h"
#include "sj_pid.h"      // 1. 确认包含正确的 sj_pid.h
#include "main.h"        // 2. 确认包含 main.h 以获取舵机宏
#include "sj_utils.h"    // 3. 关键！包含它以获取 fclip 函数的定义

// --- 4. 使用 sj_pid 规范来定义和初始化 PID 变量 ---
//    将定义和初始化分开，以避免编译时常量问题

// 定义全局 PID 变量
sf_pid_param_t servo_pid;
sf_pid_param_t adc_pid;   // 如果您不用电感，这个可以不初始化
sf_pid_param_t laser_pid; // 如果您不用激光，这个可以不初始化


// 暂时不需要的变量
float laser_angle = SMOTOR2_CENTER;

// --- 5. smotor_init() - 初始化硬件和 PID 变量 ---
void smotor_init(void) {
    // 初始化 PID 变量
    servo_pid = (sf_pid_param_t)SF_PID_CREATE(0.5f, 0.0f, 0.0f, 0.8f, 15.0f, 5.0f, 15.0f);
    adc_pid   = (sf_pid_param_t)SF_PID_CREATE(20.0f, 0.0f, 0.0f, 1.0f, 15.0f, 5.0f, 15.0f);
    laser_pid = (sf_pid_param_t)SF_PID_CREATE(-0.12f, 0.0f, 0.0f, 1.0f, 20.0f, 20.0f, 20.0f);
    
    // 初始化主转向舵机硬件
    // 计算中值角度
    float middle_angle = (SERVO_MOTOR_L_MAX + SERVO_MOTOR_R_MAX) / 2.0f;
    // 使用您在 main.h 中定义的宏进行硬件初始化
    pwm_init(SERVO_MOTOR_PWM, SERVO_MOTOR_FREQ, (uint32_t)SERVO_MOTOR_DUTY(middle_angle));

    // 其他舵机 (云台等)，如果您不使用，可以保持注释
    // pwm_init(SMOTOR2_PIN, SERVO_FREQ, (uint32_t)servo_duty(SMOTOR2_CENTER));
    // pwm_init(SMOTOR3_PIN, SERVO_FREQ, (uint32_t)servo_duty(SMOTOR3_CENTER));
}

// --- 6. 声明 smotor_control 函数，即使我们不直接调用它 ---
//    这是为了与 smotor.h 中的声明匹配，避免编译错误
void smotor1_control(uint32_t duty) {
    pwm_set_duty(SERVO_MOTOR_PWM, duty);
}
void smotor2_control(uint32_t duty) { /* 占位符 */ }
void smotor3_control(uint32_t duty) { /* 占位符 */ }


// --- 7. servo_duty() - 适配 main.h 中的宏 ---
//    这个函数保持不变，它已经很完美了
uint32_t servo_duty(float angle) {
    return (uint32_t)SERVO_MOTOR_DUTY(angle);
}

// ====================== 请用下面的【正确】代码替换 ======================

// --- 8. steering_control() - 最终给主循环调用的接口 ---
void steering_control(float angle) 
{
    // 定义舵机的最大偏转角度 (例如，左右各14.5度)
    // 这个值应该和你主循环里的限幅值保持一致
    const float MAX_STEER_ANGLE = 14.5f;

    // a. 将传入的 pid 输出角度进行限幅，确保安全
    angle = fclip(angle, -MAX_STEER_ANGLE, MAX_STEER_ANGLE);

    // b. 【核心修正】进行线性映射 (Interpolation)
    //    将 [-14.5, 14.5] 的角度范围，映射到 [SERVO_MOTOR_R_MAX, SERVO_MOTOR_L_MAX] 的占空比范围
    //    我们知道: angle = -14.5 -> duty = 83 (R_MAX)
    //             angle =  14.5 -> duty = 98 (L_MAX)

    float center_duty = (SERVO_MOTOR_L_MAX + SERVO_MOTOR_R_MAX) / 2.0f; // 中值占空比
    float range_duty = (SERVO_MOTOR_L_MAX - SERVO_MOTOR_R_MAX) / 2.0f; // 占空比变化范围的一半

    // 映射公式: final_duty = 中值 + (归一化角度 * 范围)
    // (angle / MAX_STEER_ANGLE) 得到一个 [-1, 1] 的归一化值
    float final_duty = center_duty + (angle / MAX_STEER_ANGLE) * range_duty;
    
    // c. 将计算出的占空比值转换为PWM硬件需要的数据格式
    uint32_t duty_to_set = servo_duty(final_duty);

    // d. 设置舵机PWM
    smotor1_control(duty_to_set);
}

// ====================== 替换结束 ======================